import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros
"""
创建订阅对象
组织被转换的坐标点
转换逻辑实现调用tf封装的算法
输出结果
"""
rospy.init_node("static_sub")

#创建缓存对象
buffer=tf2_ros.Buffer()

sub=tf2_ros.TransformListener(buffer)
ps=tf2_geometry_msgs.PointStamped()
ps.header.stamp=rospy.Time.now()
ps.header.frame_id="laser"
ps.point.x=2.0
ps.point.y=3.0
ps.point.z=5.0
rate=rospy.Rate(10)
while not rospy.is_shutdown():
    try:
        ps_out=buffer.transform(ps,"base_link")
        rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f）,参考坐标系：%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps_out.header.frame_id)
    except Exception as ee:
        rospy.logwarn("错误提示%s",ee)
    rate.sleep()